#include "Ingenicmotor.h"
#include "SdkMotor.h"

#define DEF_TAG             "Motor"

#define DEF_MOTOR_ANGLE_MAX         3600
#define DEF_MOTOR_ANGLE_MIN         (-1600)
#define DEF_MOTOR_TURN_VAL          180
#define DEFULAT_CFG_VALUE           (-1)

#define MOTOR "/dev/motor"
#define DEF_MOTOR_SPEED             1200        //频率 单位时间/1秒内的步进总数

int g_SdkMotor_Fd[SDK_MOTOR_MAX_NUM];
//struct motor_message g_motor_message;


void Ext_SdkMotor_Init(void)
{
    memset(g_SdkMotor_Fd,0,sizeof(g_SdkMotor_Fd));

    g_SdkMotor_Fd[SDK_MOTOR_0] = open(MOTOR, 0);
    if (g_SdkMotor_Fd[SDK_MOTOR_0] < 0)
    {
        perror("fail to open motor device\n");
    }

    unsigned long speed_init = DEF_MOTOR_SPEED;
    if (ioctl(g_SdkMotor_Fd[SDK_MOTOR_0], MOTOR_SPEED, &speed_init))
    {
        perror("MOTOR_CFG fail\n");
    }
}

void SdkMotor_Check_Status(int num)
{
    int ret = 0;
    struct motor_message msg;

    do {
        usleep(10 * 1000);
        ret = ioctl(g_SdkMotor_Fd[num], MOTOR_GET_STATUS, &msg);
        if (ret)
        {
            PRT_ERR(DEF_TAG,"get status fail\n");
            break;
        }
    } while(msg.status == MOTOR_IS_RUNNING);
}

void Ext_SdkMotor_Wait_Stop(int num)
{
    SdkMotor_Check_Status(num);
}

void Ext_SdkMotor_Up(int num)
{
#if CMAKE_SUPPORT_PTZ
    struct motors_steps motors_steps;
    motors_steps.x = 0;
    motors_steps.y = DEF_MOTOR_UD_TURN_VAL;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
    {
        PRT_ERR(DEF_TAG,"turn up fail\n");
    }
#endif
}

void Ext_SdkMotor_Down(int num)
{
#if CMAKE_SUPPORT_PTZ
    struct motors_steps motors_steps;
    motors_steps.x = 0;
    motors_steps.y = -DEF_MOTOR_UD_TURN_VAL;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
    {
        PRT_ERR(DEF_TAG,"turn down fail\n");
    }
#endif
}

void Ext_SdkMotor_TurnLeft(int num)
{
    struct motors_steps motors_steps;
    motors_steps.x = DEF_MOTOR_TURN_VAL;
    motors_steps.y = 0;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
    {
        PRT_ERR(DEF_TAG,"turn left fail\n");
    }
}

void Ext_SdkMotor_TurnRight(int num)
{
    struct motors_steps motors_steps;
    motors_steps.x = -DEF_MOTOR_TURN_VAL;
    motors_steps.y = 0;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
    {
        PRT_ERR(DEF_TAG,"turn right fail\n");
    }
}

void Ext_SdkMotor_TurnVal(int num,int val)
{
    struct motors_steps motors_steps;
    motors_steps.x = val;
    motors_steps.y = 0;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
    {
        PRT_ERR(DEF_TAG,"turn val fail\n");
    }
}

void Ext_SdkMotor_Stop(int num)
{
    int distance = 0;

    if (ioctl(g_SdkMotor_Fd[num], MOTOR_STOP, &distance))
    {
        PRT_ERR(DEF_TAG,"stop fail\n");
    }
}

void Ext_SdkMotor_Init_Center(int num)
{

    switch(num)
    {
    case SDK_MOTOR_0 :
    {
        int distance = DEF_MOTOR_ANGLE_MAX;
        struct motors_steps motors_steps;

        motors_steps.y = 0;
        motors_steps.x = distance;
        if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
        {
            PRT_ERR(DEF_TAG,"motor angle max fail\n");
        }
        SdkMotor_Check_Status(num);

        distance = DEF_MOTOR_ANGLE_MIN;
        motors_steps.x = distance;
        if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
        {
            PRT_ERR(DEF_TAG,"motor angle min fail\n");
        }
        SdkMotor_Check_Status(num);
    }
        break;

    case SDK_MOTOR_1 :
    {
#if CMAKE_SUPPORT_PTZ
        struct motors_steps motors_steps;
        motors_steps.x = 0;

        int distance = DEF_MOTOR_UD_ANGLE_MAX;

        motors_steps.y = distance;
        if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
        {
            PRT_ERR(DEF_TAG,"motor angle max fail\n");
        }

        SdkMotor_Check_Status(num);
        distance = DEF_MOTOR_UD_ANGLE_MIN;
        motors_steps.y = distance;
        if (ioctl(g_SdkMotor_Fd[num], MOTOR_MOVE, &motors_steps))
        {
            PRT_ERR(DEF_TAG,"motor angle min fail\n");
        }
        SdkMotor_Check_Status(num);
#endif
    }
        break;

    default:
        break;
    }
}

void Ext_SdkMotor_Destroy(void)
{
    int i = 0;
    for(i = 0;i < SDK_MOTOR_MAX_NUM;i++)
    {
        if (g_SdkMotor_Fd[i] > 0)
        {
            close(g_SdkMotor_Fd[i]);
        }
    }
}
